/*
 * @Description: ndt匹配
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:09:39
 * @LastEditTime: 2021-10-27 11:29:50
 * @LastEditors: Sang Hao
 */
#ifndef LIDAR_SLAM_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_
#define LIDAR_SLAM_MODELS_REGISTRATION_NDT_REGISTRATION_HPP_ 

#include <pcl/registration/ndt.h>
#include "lidar_slam/models/registration/registration_interface.hpp"

namespace lidar_slam {
class NDTRegistration: public RegistrationInterface{
private:
	/* data */
	pcl::NormalDistributionsTransform<CloudData::POINT, CloudData::POINT>::Ptr ndt_ptr_;
private:
	bool SetRegistrationParam(float res, float step_size, float trans_eps, int max_iter);
public:
	NDTRegistration(const YAML::Node& node);
	NDTRegistration(float res, float step_size, float trans_eps, int max_iter);
	bool SetInputTarget(const CloudData::CLOUD_PTR& input_target) override;
	
	bool ScanMatch(	const CloudData::CLOUD_PTR& input_source, 
					const Eigen::Matrix4f& predict_pose,
					CloudData::CLOUD_PTR& result_cloud_ptr,
					Eigen::Matrix4f& result_pose) override;
	float GetFitnessScore() override;
};
}

#endif